{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# setup\n",
    "from bokeh.plotting import figure, show, output_notebook\n",
    "from bokeh.models import (\n",
    "    ColumnDataSource, Range1d, DataRange1d, DatetimeAxis,\n",
    "    TickFormatter, DatetimeTickFormatter, FuncTickFormatter,\n",
    "    Grid, Legend, Plot, BoxAnnotation, Span, CustomJS, Rect, Circle, Line,\n",
    "    HoverTool, BoxZoomTool, PanTool, WheelZoomTool,\n",
    "    WMTSTileSource, LabelSet\n",
    "    )\n",
    "from bokeh.models.widgets import DataTable, DateFormatter, TableColumn, Div\n",
    "\n",
    "import numpy as np\n",
    "import sys\n",
    "import os\n",
    "\n",
    "sys.path.append(os.path.join(os.getcwd(), 'plot_app'))\n",
    "from pyulog import *\n",
    "from pyulog.px4 import *\n",
    "from plotting import *\n",
    "from config import *\n",
    "from notebook_helper import *\n",
    "\n",
    "output_notebook()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "file_name = '' # TODO: fill in file name\n",
    "ulog = ULog(file_name)\n",
    "data = ulog.data_list\n",
    "px4_ulog = PX4ULog(ulog)\n",
    "px4_ulog.add_roll_pitch_yaw()\n",
    "use_downsample = False\n",
    "\n",
    "print(\"message names: {:}\".format(sorted([d.name for d in data])))\n",
    "print_ulog_info(ulog)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "use_downsample = True # you may want to activate this for large logs (But you will not see all samples when zooming in)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# load a dataset to inspect the field names and types\n",
    "sensor_data = [ elem for elem in data if elem.name == 'vehicle_local_position' and elem.multi_id == 0][0]\n",
    "types_list = [(f.type_str, f.field_name) for f in sensor_data.field_data]\n",
    "for ftype, fname in types_list: print(\"{:10s} {:}\".format(ftype, fname))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "plot = plot_map(ulog, plot_config) # gps map\n",
    "if plot != None: show(plot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# plot raw acceleration sensor data\n",
    "data_plot = DataPlot(data, plot_config, 'sensor_combined',\n",
    "        y_axis_label = '[m/s^2]', title = 'Raw Acceleration', plot_height = 'normal')\n",
    "data_plot.add_graph(['accelerometer_m_s2[0]', 'accelerometer_m_s2[1]', 'accelerometer_m_s2[2]'],\n",
    "                    colors3, ['x', 'y', 'z'], use_downsample=use_downsample)\n",
    "show(data_plot.finalize())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# a more complex plot with multiple datasets\n",
    "data_plot = DataPlot(data, plot_config, 'vehicle_magnetometer', y_start=0, title = 'Thrust and Magnetic Field',\n",
    "                     plot_height='normal')\n",
    "data_plot.add_graph([lambda data: ('len_mag', \n",
    "    np.sqrt(data['magnetometer_ga[0]']**2 + data['magnetometer_ga[1]']**2 + data['magnetometer_ga[2]']**2))],\n",
    "    colors2[0:1], ['Norm of Magnetic Field'], use_downsample=use_downsample)\n",
    "\n",
    "data_plot.change_dataset('actuator_controls_0')\n",
    "data_plot.add_graph([lambda data: ('thrust', data['control[3]'])], colors2[1:2], ['Thrust'],\n",
    "                    use_downsample=use_downsample)\n",
    "\n",
    "show(data_plot.finalize())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# plot low-pass filtered raw acceleration sensor data\n",
    "from scipy.signal import butter, lfilter\n",
    "cur_data = ulog.get_dataset('sensor_combined').data\n",
    "t = cur_data['timestamp']\n",
    "\n",
    "fs = len(t) / ((t[-1]-t[0])/1e6) # sample rate [Hz]\n",
    "cutoff = 10 # cutoff freq [Hz]\n",
    "order = 5\n",
    "B, A = butter(order, cutoff / (fs / 2), btype='low') # Butterworth low-pass\n",
    "filtered_signal = lfilter(B, A, cur_data['accelerometer_m_s2[0]'])\n",
    "\n",
    "p = figure(plot_width = 800, active_scroll='wheel_zoom')\n",
    "p.line(t, cur_data['accelerometer_m_s2[0]'], color='red', alpha = 0.5)\n",
    "p.line(t, filtered_signal, color='blue', alpha = 0.8)\n",
    "show(p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# get the raw acceleration\n",
    "sensor_combined = ulog.get_dataset('sensor_combined').data\n",
    "ax = sensor_combined['accelerometer_m_s2[0]']\n",
    "ay = sensor_combined['accelerometer_m_s2[1]']\n",
    "az = sensor_combined['accelerometer_m_s2[2]']\n",
    "t = sensor_combined['timestamp']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# and plot it\n",
    "p = figure(plot_width = 800, active_scroll='wheel_zoom')\n",
    "p.line(t, ax, color='red', alpha = 0.8, legend=\"accel x\")\n",
    "p.line(t, ay, color='green', alpha = 0.8, legend=\"accel y\")\n",
    "p.line(t, az, color='blue', alpha = 0.8, legend=\"accel z\")\n",
    "show(p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# select a window\n",
    "dt = (t[-1]-t[0]) / len(t) / 1e6 # delta t in seconds\n",
    "start_index = int(20 / dt) # select start of window (seconds): make sure the vehicle is hovering at that point\n",
    "window_len_s = 3 # window length in seconds\n",
    "tw=t[start_index:int(start_index+window_len_s/dt)]\n",
    "axw=ax[start_index:int(start_index+window_len_s/dt)]\n",
    "ayw=ay[start_index:int(start_index+window_len_s/dt)]\n",
    "azw=az[start_index:int(start_index+window_len_s/dt)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# and plot it\n",
    "p = figure(plot_width = 800, active_scroll='wheel_zoom')\n",
    "p.line(tw, axw, color='red', alpha = 0.8, legend=\"accel x\")\n",
    "p.line(tw, ayw, color='green', alpha = 0.8, legend=\"accel y\")\n",
    "p.line(tw, azw, color='blue', alpha = 0.8, legend=\"accel z\")\n",
    "show(p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# FFT frequency plot\n",
    "import scipy\n",
    "import scipy.fftpack\n",
    "from scipy import pi\n",
    "\n",
    "FFT_x = abs(scipy.fft(axw))\n",
    "FFT_y = abs(scipy.fft(ayw))\n",
    "FFT_z = abs(scipy.fft(azw))\n",
    "\n",
    "freqs = scipy.fftpack.fftfreq(len(axw), dt)\n",
    "\n",
    "p = figure(plot_width = 800, active_scroll='wheel_zoom')\n",
    "p.line(freqs,20*scipy.log10(FFT_x), color='red', alpha = 0.8, legend=\"x\")\n",
    "p.line(freqs,20*scipy.log10(FFT_y), color='green', alpha = 0.8, legend=\"y\")\n",
    "p.line(freqs,20*scipy.log10(FFT_z), color='blue', alpha = 0.8, legend=\"z\")\n",
    "p.legend.click_policy=\"hide\"\n",
    "show(p)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### spectogram"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "window = 'hann'\n",
    "window_length = 256\n",
    "noverlap = 128\n",
    "\n",
    "data_set = {'ax': ax, 'ay': ay, 'az': az}\n",
    "legend = ['X','Y''Z']\n",
    "\n",
    "fs = 250\n",
    "\n",
    "# calculate the spectogram\n",
    "psd = dict()\n",
    "for key, val in data_set.items():\n",
    "    f, t, psd[key] = scipy.signal.spectrogram(\n",
    "        val,fs=fs, window=window, nperseg=window_length, noverlap=noverlap, scaling='density')\n",
    "    \n",
    "# sum all psd's\n",
    "key_it = iter(psd)\n",
    "sum_psd = psd[next(key_it)]\n",
    "for key in key_it:\n",
    "    sum_psd += psd[key]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### plot with bokeh"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def plot_spec(t, f, psd, legend):\n",
    "    color_mapper = LinearColorMapper(palette=viridis(256),low=-80, high=0)\n",
    "\n",
    "    im = [10 * np.log10(psd)]\n",
    "    p = figure(title='Acceleration Power Spectral Density ' + legend + ' [dB]',\n",
    "        plot_width=800,x_range=(t[0], t[-1]),y_range=(f[0], f[-1]),\n",
    "        x_axis_label='Time',y_axis_label='[Hz]',toolbar_location='above')\n",
    "    p.image(image=im, x=t[0], y=f[0],dw=(t[-1]-t[0]), dh=(f[-1]-f[0]),color_mapper=color_mapper)\n",
    "    color_bar = ColorBar(color_mapper=color_mapper,\n",
    "                         major_label_text_font_size=\"5pt\",\n",
    "                         ticker=BasicTicker(desired_num_ticks=5),\n",
    "                         formatter=PrintfTickFormatter(format=\"%f\"),\n",
    "                         label_standoff=6, border_line_color=None, location=(0, 0))\n",
    "    p.add_layout(color_bar,'right')\n",
    "    p.add_tools(BoxZoomTool(dimensions=\"width\"))\n",
    "    return p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "show(plot_spec(t,f,sum_psd,'X Y Z'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
